#ifndef _PRIMITIVE_
#define _PRIMITIVE_

#ifdef WIN32
#include <windows.h>
#endif
#ifdef MACOSX
#include <Fl/glut.H>
//#include <GL/glut.h>
#else
#include <GL/glu.h>
//#include <GL/glut.h>
#endif

#include "GeomObject.h"
#include "BodyGen.h"

#include <vl/VLd.h>

#define PRIMITIVE_MAGIC_NUMBER 1000
static int primCount=PRIMITIVE_MAGIC_NUMBER;

class Primitive{
protected:
	GeomObject *mGeom;	// geometry of the primitive

	Vec3d mDim; // dimensions for ellipsoid
	Vec3d mOffset;	// translation to the center of geometry for rendering
	//Vec3d mColor;		// color for the primitive

	Vec3d mCOM;	// COM - might be different
	Mat4d *mMassTensor;	// Mass tensor - Lagrangian dynamics
	double mMass;	// mass
	double mMassInv;	// mass	inverse to avoid division
	Mat3d *mInertia;	// Inertia matrix
	Mat3d *mInertiaInv;	// inverse Inertia matrix

	int mID; // unique id
	void init(Vec3d _trans, Vec3d _dim, double Mass);

	BodyGen *mBody;	// body to which connected (can be bodylink, rigid body etc)

	double myMax(double a, double b, double c);	// can be moved somewhere else

public:
	Primitive();
	Primitive(double mass);
	Primitive(Vec3d _mOffset, Vec3d _mDim, double Mass);
	virtual ~Primitive(){
		if(mGeom) delete mGeom;
		if(mMassTensor!=NULL) delete mMassTensor;
		if(mInertia!=NULL) delete mInertia;
		if(mInertiaInv!=NULL) delete mInertiaInv;
	}
	void draw(){
		mGeom->draw(Vec4d(1,0,0,0), true);
	}
	void draw(Vec4d _color, bool _default){
		mGeom->draw(_color, _default);
	}
	bool isInside(Vec3d _pt){	// decides if the point is inside
		return mGeom->isInside(_pt);
	}

	virtual Mat4d* getMassTensor()=0;	// xxT
	virtual Mat3d* getInertia();		// xTxI - xxT
	virtual inline Mat3d* getInertiaInv(){
		if(mInertiaInv==NULL){
			mInertiaInv = new Mat3d(vl_0);
			*mInertiaInv = inv(*getInertia());
		}
		return mInertiaInv;
	}
	virtual void setMassTensorFromInertia(double mat[3][3]);

	inline void setColor(Vec3d _color){mGeom->setColor(Vec4d(_color,1.0));}
	inline void setCOM(Vec3d _com){mCOM=_com;}

	virtual inline void setOffset(Vec3d _offset){
		mOffset=_offset;
		mGeom->translate(_offset);
	}
	virtual inline void setDim(Vec3d _dim){
		mDim=_dim;
		mGeom->setExtent(_dim);
	}
	inline Vec3d getDim(){return mDim;}
	
	inline double getMass(){return mMass;}
	inline double getMassInv(){return mMassInv;}
	//inline Vec3d getColor(){return mColor;}
	inline Vec3d getOffset(){return mOffset;}
	inline Vec3d getCOM(){return mOffset;}
	inline int getID(){return mID;}
	inline double getVolume(){return mGeom->getVolume();}

	// set some body to which it belongs 
	inline void setBody(BodyGen *b){mBody=b;}
	inline BodyGen* getBody(){return mBody;}
	void setMass(double _m);
	void setInertia(double inertia[3][3]);
	inline GeomObject* getGeom(){return mGeom;}
};

#endif
